﻿#include "algorithmodule.h"

AlgorithModule::AlgorithModule()
{

}
//寻找圆心
Point2f AlgorithModule::findCircle(Mat gray)
{
    Mat binary;
    //cvtColor(gray,gray,COLOR_BGR2GRAY);//灰度化
    //GaussianBlur(gray,gray,Size(9,9),2,2);//滤波
    threshold(gray,binary,40,255,THRESH_BINARY);//自适应二值化
    imwrite("c:/1.png",binary);
    //轮廓检测
    vector<vector<Point>> contours;//轮廓
    vector<Vec4i> hierarchy;//存放轮廓结构变量
    findContours(binary,contours,hierarchy,RETR_TREE,CHAIN_APPROX_SIMPLE,Point());
    //寻找轮廓的外接矩阵
//    for(int n=0;n<contours.size();n++) {
//          //最小外接矩阵
//        RotatedRect rrect= minAreaRect(contours[n]);
//        Point2f points[4];
//        rrect.points(points);//读取最小外接矩阵的四个顶点
//        Point2f cpt=rrect.center;//最小外接矩阵的中心
//        //绘制旋转矩阵与中心位置
//        for(int i=0;i<4;i++){
//            if(i==3){
//                line(image,points[i],points[0],Scalar(0,0,255,255),2,8,0);
//                break;
//            }
//            line(image,points[i],points[i+1],Scalar(0,0,255,255),2,8,0);
//        }
//        //绘制矩阵中心
//        circle(image,cpt,4,Scalar(0,0,255,255),-1,8,0);
//    }
    if(contours.size() == 0)
        return Point2f(-1,-1);
    RotatedRect rrect= minAreaRect(contours[0]);
    return rrect.center;
}
/*
函数功能：激光中心线点集提取
输入@img：灰度图像
返回：点集
备注：2020.5.13
重心法：将图像转为灰度图并二值化，把灰度值看作质量，每一行的重心作为激光线的中心
*/
std::vector<cv::Point> AlgorithModule::getGrayCenterPoints_gravity(cv::Mat img1)
{
    Mat img = img1.clone();
    std::vector<cv::Point> centerPoints;
    int h = img.rows, w = img.cols;
    medianBlur(img, img, 5);
    cv::threshold(img, img, 100, 255, cv::THRESH_BINARY);
    for (int i = 0; i < img.rows; i++)
    {
        cv::Point point;
        long long sxp = 0, sp = 0, xpos;
        for (int j = 0; j < img.cols; j++)
        {
            sxp += (j*(long long)(img.at<uint8_t>(i, j)));
            sp += (long long)(img.at<uint8_t>(i, j));
        }
        if (sp)
        {
            xpos = sxp / sp;//xpos即为每一行的重心坐标
            //xpos就是这一行的重心纵坐标
            //Laser_Line.at<uint8_t>(i, xpos) = 255;

            point.y = i;
            point.x = xpos;

            centerPoints.push_back(point);
            //circle(img, point, 2, Scalar(0, 255, 120), -1);//画点，其实就是实心圆
        }else{
            if(centerPoints.size() ==0)
            {
                cv::Point point;
                point.y = i;
                point.x = 0;
                centerPoints.push_back(point);
            }else{
                centerPoints.push_back(centerPoints.at(i-1));
            }
        }
    }

    return centerPoints;
}
/*
函数功能：激光中心线点集提取
输入@img：灰度图像
返回：点集
备注：2020.5.13
阈值法：
从图像的左右两侧向中间寻找激光条的左右边界，然后找到左右边界的中点即为激光线的中心
*/
std::vector<cv::Point> AlgorithModule::getGrayCenterPoints_threshold(cv::Mat img1)
{
    Mat img = img1.clone();
    std::vector<cv::Point> centerPoints;
    int h = img.rows, w = img.cols;
    int index1 = 0;
    int index2 = 0;
    int threshod = 170;
    for (int i = 0; i < h; i++)
   {
        cv::Point point;
       //从左侧开始向右查找
       for (int j = 0; j < w; j++)
       {
           if ((img.at<uint8_t>(i, j) > threshod))
           {
               index1 = j;
               //cout << i << " index1: " << index1 << " ";
               break;
           }
       }
       //从右侧开始向左查找
       for (int j = w - 1; j >= 0; j--)
       {
           if ((img.at<uint8_t>(i, j) > threshod))
           {
               index2 = j;
               //cout << "index2: " << index2 << endl;
               break;
           }
       }
       if ((index1 != 0) && (index2 != 0) && (abs(index1 - index2) < 50))
       {
           point.y = i;
           point.x = (index1 + index2) / 2;
           centerPoints.push_back(point);
       }else{
           if(centerPoints.size() ==0)
           {
               cv::Point point;
               point.y = i;
               point.x = 0;
               centerPoints.push_back(point);
           }else{
               centerPoints.push_back(centerPoints.at(i-1));
           }
       }
       index1 = 0;
       index2 = 0;
   }

    return centerPoints;
}
/*
函数功能：激光中心线点集提取
输入@img：灰度图像
返回：点集
备注：2020.5.13
极值法：
首先将图像转化为灰度图，然后按行寻找每一行的亮度最大值，在这里还加入了一个阈值threshod，也可以不加
*/
std::vector<cv::Point> AlgorithModule::getGrayCenterPoints_peak(cv::Mat img1)
{
    Mat img = img1.clone();
    std::vector<cv::Point> centerPoints;
    int h = img.rows, w = img.cols;
    int index = 0;
    //这里设置一个阈值，每一行的最大值不一定是激光线，大于这个阈值才认为是激光线
    int threshod = 170;
    for (int i = 0; i < h; i++)
    {
        cv::Point point;
        double max_row = 0;
        for (int j = 0; j < w; j++)
        {
            if (img.at<uint8_t>(i, j) > max_row)
            {
                max_row = img.at<uint8_t>(i, j);
                index = j;
            }
        }
        if (max_row > threshod)
        {
            point.y = i;
            point.x = index;
            centerPoints.push_back(point);
        }else{
            if(centerPoints.size() ==0)
            {
                cv::Point point;
                point.y = i;
                point.x = 0;
                centerPoints.push_back(point);
            }else{
                centerPoints.push_back(centerPoints.at(i-1));
            }
        }
    }

    return centerPoints;
}
/*
 * 拟合直线
*/
void AlgorithModule::fitLine(std::vector<cv::Point> points, Vec4f &line)
{
    // 拟合直线
    cv::fitLine(points, line, cv::DIST_L2, 0, 0.01, 0.01);
}

/*
函数功能：显示拟合直线结果
输入@img：图像
输入@a,b：直线方程参数
备注：2020.4.18
*/
void AlgorithModule::showLineFitting(cv::Mat& img, Vec4f line)
{
    //获取点斜式的点和斜率
    cv::Point point0;
    point0.x = line[2];
    point0.y = line[3];

    double k = line[1] / line[0];

    //计算直线的端点(y = k(x - x0) + y0)
    cv::Point point1, point2;
    point1.x = 0;
    point1.y = k * (0 - point0.x) + point0.y;
    point2.x = img.cols;
    point2.y = k * (img.cols - point0.x) + point0.y;

    // 绘制直线
    cv::line(img, point1, point2, cv::Scalar(0, 0, 255), 2, 8, 0);

//    double cos_theta = line[0];
//    double sin_theta = line[1];
//    double x0 = line[2], y0 = line[3];

//    double k = sin_theta / cos_theta;
//    double b = y0 - k * x0;

//    double x = 0;
//    double y = k * x + b;

//    double x1 = img.cols;
//    double y1 = k * x1 + b;

//    cv::line(img, Point(x1,y1), Point(x,y), cv::Scalar(0, 0,255),2, 8, 0);
}
//用拟合的直线，计算y=kx+b中的k和b
void AlgorithModule::get_k_b(Vec4f line,double &k,double &b)
{
    //获取点斜式的点和斜率
    cv::Point point0;
    point0.x = line[2];
    point0.y = line[3];

    k = line[1] / line[0];
    //计算直线的端点(y = k(x - x0) + y0)
    b = k * (0 - point0.x) + point0.y;
}
//计算点到直线的距离
double AlgorithModule::get_dis(Vec4f line,Point pointP)
{
    //获取点斜式的点和斜率
    cv::Point point0;
    point0.x = line[2];
    point0.y = line[3];

    double k = line[1] / line[0];

    //计算直线的端点(y = k(x - x0) + y0)
    cv::Point point1, point2;
    point1.x = 0;
    point1.y = k * (0 - point0.x) + point0.y;
    point2.x = 640;
    point2.y = k * (640 - point0.x) + point0.y;

    //求直线方程
    int A = 0, B = 0, C = 0;
    A = point1.y - point2.y;
    B = point2.x - point1.x;
    C = point1.x*point2.y - point1.y*point2.x;
    //代入点到直线距离公式
    double distance = 0;
    double chushu = ((double)(A*pointP.x + B*pointP.y + C)) ;
    double beichushu = ((double)sqrt(pow(A,2) + pow(B,2)));
    distance = chushu/ beichushu;
    return distance;
}
//滤波
vector<double> AlgorithModule::movingAverage(const vector<double>& input, int windowSize)
{
    vector<double> output(input.size() - windowSize + 1);
    for (int i = 0; i < output.size(); ++i) {
        double sum = 0.0;
        for (int j = i; j < i + windowSize; ++j) {
            sum += input[j];
        }
        output[i] = sum / windowSize;
    }
    return output;
}
//获取某个位置的线宽
//Mat mat原始图像,Vec4f line检测出的直线,int row需要检测的横向位置,
//int &start线宽的开始位置,int &end线宽的结束位置
int AlgorithModule::getLineWidth(Mat mat,Vec4f line,int row,Point &start,Point &end)
{
    end.y = row;
    start.y = row;
    cv::Point point = getCross( line,row);
    //qDebug()<<point.x<<point.y<<"111"<<mat.at<uint8_t>(point.x,point.y);
    int width=0;
    for(int i=point.x;i<mat.cols;i++){
        if(mat.at<uint8_t>(row,i)>100){
            width++;
            end.x = i;
        }else if(mat.at<uint8_t>(row,i) < 100){
            break;
        }
    }
    for(int i=point.x;i>0;i--){
        if(mat.at<uint8_t>(row,i)>100){
            width++;
            start.x = i;
        }else if(mat.at<uint8_t>(row,i) < 100){
            break;
        }
    }
    return width;
}
//获取交点
cv::Point AlgorithModule::getCross(Vec4f line,int row)
{
    //获取点斜式的点和斜率
    cv::Point point0;
    point0.x = line[2];
    point0.y = line[3];

    double k = line[1] / line[0];

    //计算直线的端点(y = k(x - x0) + y0)
    cv::Point point1;
    point1.y = row;
    point1.x = (point1.y- point0.y)/k + point0.x;

    return point1;
}
